#define _USE_MATH_DEFINES

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <signal.h>
#include <time.h>

#include "maestro.h"
#include "compass.h"
#include "gotemp.h"
#include "gps_boat.h"
#include "config.h"
#include <gps.h>
#include "main.h"

#define MAX_LINE 256
#define INT_TICKS 120
#define TICK_INTERRUPT false

int maestro;
int compass;
FILE * logFile;
FILE * statusFile;


int main(){
	signal(SIGINT, signal_handler);

	maestro = maestro_connect();

	compass = compass_connect();
	fprintf(stderr, "M:%d C:%d\n", maestro, compass);

	if ((maestro < 0) || (compass < 0))
	{
		fprintf(stderr, "Connection error, exiting.\n");
		exit(1);
	}


	double retHeading, retPitch, retRoll, retTemp;
	bool goodData;
	get_compass(&retHeading, &retPitch, &retRoll, &retTemp, &goodData);
	fprintf(stderr, "Heading: %f, Pitch: %f, Roll: %f, Temp: %f, goodData: %d\n", retHeading, retPitch, retRoll, retTemp, goodData);


	gps_connect();

	
	
	fprintf(stderr, "*************Devices connected, loading configuration.\n");

	loadConfig();
	fprintf(stderr, "*************Configuration loaded.\n");


	char expFilename[50];
	sprintf(expFilename, "Outputs/experiment_%ld.txt", time(NULL));

	statusFile = fopen("Outputs/status.txt", "w");
	
	while(1)
	{	
		fprintf(stderr, "Waiting for mission file\n");

		while(checkInterrupt() == false){
			usleep(1000000);
			fprintf(stderr, "Mission file not found, waiting 1 second\n");		
		}
	
		int renam = rename( "Commands/mission.txt", "Commands/mission-running.txt");
		FILE* fp = fopen("Commands/mission-running.txt", "r");

		logFile = fopen(expFilename, "w");
		
		
		// Mission file opened
		fprintf(stderr, "Mission file found, parsing.\n");
		char str[MAX_LINE];
		int interrupt = 0;
		while ((fgets(str, MAX_LINE, fp) != NULL) && (interrupt != 1))
		{
			//fprintf(stderr, "String: '%s' returned\n", str);
			char *pch;
			pch = strchr(str, '#');
			if (pch != NULL)
			{
				//fprintf(stderr, "Comment: %s", str);
			}
			else
			{
				int empty = strcmp(str, "\n");
				if (empty == 1)
				{
					char function[50];
					char args[50];

					sscanf(str, "%s \t %[^\n] \n", function, args);
					
					fprintf(stderr, "%s\n", args);
					if (!strcmp(function, "goToWaypoint"))
					{
						double lat = 1;
						double lon = 1;
						double precision = 1;// lon, precision;
						sscanf(args, "[ %lf, %lf, %lf ]", &lat, &lon, &precision);
						//fprintf(stderr, "%f\n", lat);
						fprintf(stderr, "Going to waypoint: %f,%f with %f precision\n", lat, lon, precision);
						interrupt = goToWaypoint(lat, lon, precision);
					}
					else if(!strcmp(function, "goToHeading"))
					{
						double desHeading;
						sscanf(args, "[%lf]", &desHeading);
						fprintf(stderr, "Going to desired heading of: %f\n", desHeading);
						goToHeading(desHeading);
					}
					else if(!strcmp(function, "halt"))
					{
						halt();
					}
					else
					{
						fprintf(stderr, "cannot parse %s\n", function);
					}

				}
				else
				{
					//fprintf(stderr, "Empty line\n");
				}
			}
		}
		fclose(pFile);

		fclose(fp);

		int result = rename( "Commands/mission-running.txt", "Commands/mission-finished.txt");
		if (result == 0)
		{
			fprintf(stderr, "Mission completed, file renamed.\n");
		}
		else
		{
			fprintf(stderr, "Mission completed, file could not be renamed. Permission error?\n");
			exit(0);
		}
	}
	return 0;
}


int halt()
{
	fprintf(stderr, "Stopping.\n");
	setTarget(maestro, getRudder(), 1500);
	setTarget(maestro, getThrottle(), 1500);
	return 0;
}



int goToHeading (double desiredHeading)
{

	double retHeading, retPitch, retRoll, retTemp;
	bool goodData;
	get_compass(&retHeading, &retPitch, &retRoll, &retTemp, &goodData);
	fprintf(stderr, "Heading: %lf, Pitch: %lf, Roll: %lf, Temp: %lf, goodData: %d\n", retHeading, retPitch, retRoll, retTemp, goodData);
	double difference = desiredHeading - retHeading;

	while(abs(difference) > 5)
	{
		fprintf(stderr, "Heading: %lf, Pitch: %lf, Roll: %lf, Temp: %lf, goodData: %d\n", retHeading, retPitch, retRoll, retTemp, goodData);
		if (difference < -180)
			difference = difference + 360;
		if (difference > 180)
			difference = difference - 360;

		unsigned int command = 1500 - getRudderCoeff()*difference;
		fprintf(stderr, "Desired heading: %lf, current heading: %lf, Setting rudder (%d) pos: %d\n", desiredHeading, retHeading, getRudder(), command);
		setTarget(maestro, getRudder(), command);
		usleep(100000);
		get_compass(&retHeading, &retPitch, &retRoll, &retTemp, &goodData);
		difference = desiredHeading - retHeading;
	}
	setTarget(maestro, getRudder(), 1500);
	fprintf(stderr, "Heading achieved. \n");
	return 0;
}



/* This function goes to a waypoint with a certain precision. Currently, it first aligns the heading, then goes to the waypoint*/
int goToWaypoint (double latitude, double longitude, double metersPrecision)
{

	fprintf(logFile, "<wayPoint>\n");


	gps_data_t current;
	gps_fetch(&current);
	
	double startTime = current.fix.time;
	
	while (current.status == 0 || ((current.fix.latitude == 180.0) && (current.fix.longitude == 0.0)))
	{
		usleep(500000);	
		gps_fetch(&current);
		fprintf(stderr, "Grabbing a GPS fix, status: %d. Sat viewed: %d. Lat: %lf, Lon: %lf\n", current.status, current.satellites_visible, current.fix.latitude, current.fix.longitude);
	}

	double distance = lawCosines(current.fix.latitude, current.fix.longitude, latitude, longitude);
	
	while (isnan(distance))
	{		
		fprintf(stderr, "GPS NaNs...waiting... %lf, %lf\n", current.fix.latitude, current.fix.longitude);
		gps_fetch(&current);
		distance = lawCosines(current.fix.latitude, current.fix.longitude, latitude, longitude);
		usleep(1000000);
	}
	fprintf(stderr, "Distance to waypoint: %lf m\n", distance);
	fprintf(logFile, "<comment>Distance to waypoint: %lf m</comment>\n", distance);
	if (distance < metersPrecision)
	{
		fprintf(stderr, "Already at waypoing.\n");
		fprintf(logFile, "<comment>Already at waypoint.</comment>\n</wayPoint>\n");
		return 0;
	}
	
	double diffLat =  current.fix.latitude - latitude;
	double diffLon =  current.fix.longitude - longitude;
	fprintf(stderr, "Diff lat: %lf, diff lon: %lf\n", diffLat, diffLon);
	
	double desiredHeading = calcHeading(current.fix.latitude, current.fix.longitude, latitude, longitude);
/*	fprintf(stderr, "Desired heading: %lf, going to heading.\n", desiredHeading);
	usleep(5000000);
	goToHeading(desiredHeading);
*/
	fprintf(stderr, "Moving to waypoint.\n");
//	usleep(5000000);


	int timerInterrupt=0;
	/* This is our main loop for waypoints */	
	while ((distance > metersPrecision) && (checkInterrupt() == false) && (timerInterrupt == 0))
	{			
		fprintf(stderr, "Distance: %lf\n", distance);
		gps_fetch(&current);
		usleep(500000);

		double temp = gotemp_read();
		fprintf(pFile, "%lf, %lf, %lf, %lf, %lf, %lf\n", current.fix.time, current.fix.latitude, current.fix.longitude, latitude, longitude, temp);

		diffLat =  current.fix.latitude - latitude;
		diffLon =  current.fix.longitude - longitude;
		
		//desiredHeading = desiredHeading = 180/M_PI * atan2(diffLon, diffLat);
		desiredHeading = calcHeading(current.fix.latitude, current.fix.longitude, latitude, longitude);		
		distance = lawCosines(current.fix.latitude, current.fix.longitude, latitude, longitude);
		
		// First let's set the rudder
		double retHeading, retPitch, retRoll, retTemp;
		bool goodData;
		get_compass(&retHeading, &retPitch, &retRoll, &retTemp, &goodData);
		double headingDiff = desiredHeading - retHeading;
		if (headingDiff < -180)
			headingDiff = headingDiff + 360;
		if (headingDiff > 180)
			headingDiff = headingDiff - 360;

		unsigned int commandRudder = 1500 - getRudderCoeff()*headingDiff;
	
		unsigned int commandThrottle = 1500 + getThrottleCoeff()*distance;
		if (commandThrottle > 2000)
			commandThrottle = 2000;


		//setTarget(maestro, getRudder(), commandRudder);		
		//setTarget(maestro, getThrottle(), commandThrottle);
		commandMix(commandRudder, commandThrottle);
		fprintf(stderr, "Desired heading: %lf, current heading: %lf\n", desiredHeading, retHeading);	
		fprintf(stderr, "Desired lat: %lf, Desired lon: %lf. Current: %lf, %lf. Distance: %lf m.\n", latitude, longitude, current.fix.latitude, current.fix.longitude, distance);	

		
		if ((TICK_INTERRUPT == true) && ((((int)(current.fix.time-startTime)) % INT_TICKS) == 0))
		{
			timerInterrupt = 1;
		}
			
	}





	if (distance < metersPrecision)
	{
		fprintf(stderr, "Waypoint achieved.\n");
	}
	else if (timerInterrupt == 1)
	{
		fprintf(stderr, "Timer interrupt.\n");
		return 1;
	}
	else
	{
		fprintf(stderr, "Interrupted.\n");
		return 1;
	}
	setTarget(maestro, getRudder(), 1500);
	setTarget(maestro, getThrottle(), 1500);
	return 0;
}

double lawCosines(double lat1, double lon1, double lat2, double lon2)
{
	double radius = 637100;	
	lat1 = lat1 * M_PI/180;
	lat2 = lat2 * M_PI/180;
	lon1 = lon1 * M_PI/180;
	lon2 = lon2 * M_PI/180;
	double sines = sin(lat1)*sin(lat2);
	double cosines = cos(lat1)*cos(lat2)*cos(lon2-lon1);
	fprintf(stderr, "Sines: %lf, Cosines: %lf\n", sines, cosines);
	fprintf(stderr, "Acos: %lf\n", acos(sines + cosines));
	return (acos(sines + cosines) * radius);
}

double calcHeading(double lat1, double lon1, double lat2, double lon2)
{
	lat1 = lat1 * M_PI/180;
	lat2 = lat2 * M_PI/180;
	lon1 = lon1 * M_PI/180;
	lon2 = lon2 * M_PI/180;
	double diffLon = lon2-lon1;
	//double diffLat = lat2-lat1;

	double y = sin(diffLon) * cos(lat2);
	double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(diffLon);
	double desiredHeading = (atan2(y, x) * 180/M_PI);
	return fmod(desiredHeading + 360.0, 360);
}

void signal_handler(int signum)
{
	halt();
	fclose(pFile);
	exit(signum);	
}

void commandMix(unsigned int rudder, unsigned int throttle)
{
	
	if (getRudderMixer() == 1.0)
	{
		int rudderDiff = abs(rudder - 1500);

		double scaleRudder = 1 - ((double)rudderDiff / 500); // Between 0 (absolute rudder) and 1 (rudder neutral)
		
		int throttleDiff = throttle - 1500;
		double throttleDiffScaled = scaleRudder * throttleDiff;
		unsigned int newThrottle = 1500 + throttleDiffScaled;
		
		setTarget(maestro, getRudder(), rudder);
		setTarget(maestro, getThrottle(), newThrottle);	
		fprintf(stderr, "Mixed, setting throttle %d (was:%d), rudder %d (was:%d). rudderDiff:%d, scaleRudder:%lf, trottleDiff:%d, throttleDiffScaled: %lf \n", newThrottle, throttle, rudder, rudder, rudderDiff, scaleRudder, throttleDiff, throttleDiffScaled );	
	}
	else
	{
		setTarget(maestro, getRudder(), rudder);
		setTarget(maestro, getThrottle(), throttle);

		fprintf(stderr, "Setting throttle %d, rudder %d. \n", rudder, throttle);	
	}

}

int checkInterrupt()
{
	FILE* fp = fopen("Commands/mission.txt", "r");
	if (fp == NULL)
		return 0;
	else
		return 1;
}	
